/*! *********************************************************************************
* \addtogroup Private Profile Server
* @{
********************************************************************************** */
/*! *********************************************************************************
* Copyright (c) 2014, Freescale Semiconductor, Inc.
* Copyright 2022 NXP
* All rights reserved.
*
* \file
*
* This file is the source file for the QPP Server application
*
* SPDX-License-Identifier: BSD-3-Clause
********************************************************************************** */

/************************************************************************************
*************************************************************************************
* Include
*************************************************************************************
************************************************************************************/
/* #undef CR_INTEGER_PRINTF to force the usage of the sprintf() function provided
 * by the compiler in this file. The sprintf() function is #included from
 * the <stdio.h> file. */
#ifdef CR_INTEGER_PRINTF
#undef CR_INTEGER_PRINTF
#endif

#ifdef UWBIOT_USE_FTR_FILE
#include "uwb_iot_ftr.h"
#else
#include "uwb_iot_ftr_default.h"
#endif

#ifndef UWBIOT_APP_BUILD__DEMO_NEARBY_INTERACTION
#include "UWBIOT_APP_BUILD.h"
#endif

#ifdef UWBIOT_APP_BUILD__DEMO_NEARBY_INTERACTION

/* TLV includes */
#include "TLV_Types_i.h"

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

//#include "PWR_Configuration.h"

/* Framework / Drivers */
#include "EmbeddedTypes.h"
#include "stdio.h"
#include "driver_config.h"
#include "RNG_Interface.h"
#include "fsl_component_button.h"
#include "fsl_component_led.h"
#include "fsl_component_timer_manager.h"
#include "fsl_component_mem_manager.h"
#include "fsl_component_panic.h"
#include "fsl_component_serial_manager.h"

#include "FunctionLib.h"


/* BLE Host Stack */
#include "gatt_server_interface.h"
#include "gatt_client_interface.h"
#include "gap_interface.h"
#include "gatt_db_handles.h"


/* Profile / Services */
#include "battery_interface.h"
#include "device_info_interface.h"
#include "ble_profile_interface.h"

/* Connection Manager */
#include "ble_conn_manager.h"

#include "board.h"
#include "app.h"
#include "app_conn.h"
#include "app_advertiser.h"
//#include "demo_ble_server.h"


#include "UWBT_PowerMode.h"
#include "UWBT_Config.h"
#include "uwb_types.h"

#if (gAppNtagSupported_d)
#include "app_ntag.h"
#endif

#if gBtnSupported_d
#include "gpio_pins.h"
#include "ButtonScan.h"
#endif

#include "phOsalUwb.h"
#include "phOsalUwb_Queue.h"
#include "phTmlUwb_transport.h"



void UWB_Interrupt_ISR_Init(void);
void UWB_Interrupt_ISR(void);
extern intptr_t tlvMngQueue;
extern uint32_t mSessionId;
extern bool sr040_initialization_failed;

/************************************************************************************
*************************************************************************************
* Private macros
*************************************************************************************
************************************************************************************/
#define mAttNotifHeaderSize_c              (3) /* ATT op code + ATT handle  */
#define BLE_CONN_CB_MAX_REENTRANCE_TIMEOUT (0x2000)

/************************************************************************************
*************************************************************************************
* Private type definitions
*************************************************************************************
************************************************************************************/
typedef enum
{
#if gAppUseBonding_d
#if defined(gAppUsePrivacy_d) && (gAppUsePrivacy_d > 0)
    fastWhiteListAdvState_c,
#endif // defined(gAppUsePrivacy_d) && (gAppUsePrivacy_d > 0)
#endif // gAppUseBonding_d
    fastAdvState_c,
    slowAdvState_c,
    defaultAdvState_c
} advType_t;

typedef struct advState_tag
{
    bool_t advOn;
    advType_t advType;
} advState_t;

typedef struct appPeerInfo_tag
{
    uint8_t deviceId;
    uint8_t ntf_cfg;
} appPeerInfo_t;

/************************************************************************************
*************************************************************************************
* Private memory declarations
*************************************************************************************
************************************************************************************/
/* Adv State */
static advState_t mAdvState;
//static bool_t      mRestartAdv;
static uint32_t mAdvTimeout;
/* Service Data*/
static qppsConfig_t qppServiceConfig = {service_qpps};

static uint16_t cpHandles[1]     = {value_qpps_rx};
static uint16_t cpReadHandles[1] = {value_nearby_data};

static deviceId_t  mPeerDeviceId = gInvalidDeviceId_c;

/* Application specific data*/
static appPeerInfo_t mPeerInformation[gAppMaxConnections_c];

extern gapAdvertisingData_t         gAppAdvertisingData;
extern gapScanResponseData_t        gAppScanRspData;
extern gapAdvertisingParameters_t   gAdvParams;

static appAdvertisingParams_t mAppAdvParams = {
    &gAdvParams,
    &gAppAdvertisingData,
    &gAppScanRspData
};


#if TAG_BUILD_CFG == MANUF_TEST_BUILD || TAG_BUILD_CFG == VALIDATION_V3_BUILD
extern bool bleConnection;
#endif

/* UWB operation status */
extern QueueHandle_t mTlvMutex;

/************************************************************************************
*************************************************************************************
* Private functions prototypes
*************************************************************************************
************************************************************************************/

/* Gatt and Att callbacks */


static void BleApp_Advertise(void);
static void BleApp_ReceivedDataHandler(deviceId_t deviceId, uint8_t *aValue, uint16_t valueLength);


/* Gatt and Att callbacks */
static void BleApp_AdvertisingCallback (gapAdvertisingEvent_t* pAdvertisingEvent);
static void BleApp_ConnectionCallback (deviceId_t peerDeviceId, gapConnectionEvent_t* pConnectionEvent);
static void BleApp_GattServerCallback (deviceId_t deviceId, gattServerEvent_t* pServerEvent);

static void BleApp_Advertise(void);

static void BluetoothLEHost_Initialized(void);
static void BleApp_SerialInit(void);
static void BluetoothLEHost_GenericCallback (gapGenericEvent_t* pGenericEvent);



/************************************************************************************
*************************************************************************************
* Public functions
*************************************************************************************
************************************************************************************/

/*! *********************************************************************************
* \brief    Initializes application specific functionality before the BLE stack init.
*
********************************************************************************** */
void BluetoothLEHost_AppInit(void)
{
    /* Initialize application support for drivers */
    LedStartFlashingAllLeds();

    /* Initialize Bluetooth Host Stack */
    BluetoothLEHost_SetGenericCallback(BluetoothLEHost_GenericCallback);
    BluetoothLEHost_Init(BluetoothLEHost_Initialized);
}

/************************************************************************************
*************************************************************************************
* Private functions
*************************************************************************************
************************************************************************************/

/*! *********************************************************************************
* \brief        Configures BLE Stack after initialization. Usually used for
*               configuring advertising, scanning, white list, services, et al.
*
********************************************************************************** */

/*! *********************************************************************************
* \brief    Starts the BLE application.
*
********************************************************************************** */
void BleApp_Start(void)
{
    Led1On();

    if (mPeerDeviceId == gInvalidDeviceId_c)
    {
        /* Device is not connected and not advertising */
        if (!mAdvState.advOn)
        {
            /* Set advertising parameters, advertising to start on gAdvertisingParametersSetupComplete_c */
            BleApp_Advertise();
        }
    }

}

/*! *********************************************************************************
* \brief        Configures GAP Advertise parameters. Advertise will start after
*               the parameters are set.
*
********************************************************************************** */
static void BluetoothLEHost_Initialized(void)
{
    /* Common GAP configuration */
    BleConnManager_GapCommonConfig();

    /* Register for callbacks*/
    GattServer_RegisterHandlesForWriteNotifications(NumberOfElements(cpHandles), cpHandles);
    GattServer_RegisterHandlesForReadNotifications(NumberOfElements(cpReadHandles), cpReadHandles);
    (void)App_RegisterGattServerCallback(BleApp_GattServerCallback);

    mAdvState.advOn = FALSE;


    BleApp_Start();

}
static void BleApp_Advertise(void)
{
    /* Start advertising */
    (void)BluetoothLEHost_StartAdvertising(&mAppAdvParams, BleApp_AdvertisingCallback, BleApp_ConnectionCallback);
}

/*! *********************************************************************************
* \brief        Handles BLE generic callback.
*
* \param[in]    pGenericEvent    Pointer to gapGenericEvent_t.
********************************************************************************** */
static void BluetoothLEHost_GenericCallback (gapGenericEvent_t* pGenericEvent)
{
    /* Call BLE Conn Manager */
    BleConnManager_GenericEvent(pGenericEvent);

    switch (pGenericEvent->eventType)
    {
    case gAdvertisingSetupFailed_c:
        {
            panic(0,0,0,0);
        }
        break;
#if defined(gAppPrintLePhyEvent_c) && (gAppPrintLePhyEvent_c)
    case gLePhyEvent_c:
        if(pGenericEvent->eventData.phyEvent.phyEventType == gPhyUpdateComplete_c )
        {
            AppPrintLePhyEvent(&pGenericEvent->eventData.phyEvent);
        }
        break;
#endif
    default:
        {
            ; /* No action required */
        }
        break;
    }
}


static uint8_t howManyPeer(void)
{
    uint8_t nb_peer = 0;
    for (uint8_t i = 0; i < gAppMaxConnections_c; i++) {
        if (mPeerInformation[i].deviceId != gInvalidDeviceId_c)
            nb_peer++;
    }
    return nb_peer;
}

/*! *********************************************************************************
* \brief        Handles BLE Connection callback from host stack.
*
* \param[in]    peerDeviceId        Peer device ID.
* \param[in]    pConnectionEvent    Pointer to gapConnectionEvent_t.
********************************************************************************** */
static void BleApp_ConnectionCallback(deviceId_t peerDeviceId, gapConnectionEvent_t *pConnectionEvent)
{
    /* Connection Manager to handle Host Stack interactions */
    BleConnManager_GapPeripheralEvent(peerDeviceId, pConnectionEvent);

    switch (pConnectionEvent->eventType) {
    case gConnEvtConnected_c: {
    	mAdvState.advOn = FALSE;
#if defined(cPWR_UsePowerDownMode) && (cPWR_UsePowerDownMode)
        UWBT_PowerModeEnter(UWBT_RUN_MODE);
#endif /* cPWR_UsePowerDownMode */
        mPeerInformation[peerDeviceId].deviceId = peerDeviceId;

        if (phOsalUwb_ConsumeSemaphore_WithTimeout(mTlvMutex, BLE_CONN_CB_MAX_REENTRANCE_TIMEOUT) !=
            UWBSTATUS_SUCCESS) {
            LOG_E("Ble connection callback timeout");
        }
        if (!handleDeviceInit()) {
            LOG_E("Device init failed");
        }
        (void)phOsalUwb_ProduceSemaphore(mTlvMutex);

        /* Subscribe client*/
        (void)Qpp_Subscribe(peerDeviceId);

        LOG_I("BLE Connected to peer #%d, %d peers connected\r\n", peerDeviceId + 1, howManyPeer());
        /* Restart Advertising while max connection is not reached */
        if (howManyPeer() < gAppMaxConnections_c) {
            BleApp_Start();
        }

#if TAG_BUILD_CFG == MANUF_TEST_BUILD || TAG_BUILD_CFG == VALIDATION_V3_BUILD
        bleConnection = TRUE;
#endif
    } break;

    case gConnEvtEncryptionChanged_c: {
        PRINTF("Encrypted BLE connection\n");
    } break;

    case gConnEvtDisconnected_c: {
        /* qpps Unsubscribe client */
        Qpp_Unsubscribe();
        mPeerInformation[peerDeviceId].ntf_cfg  = QPPS_VALUE_NTF_OFF;
        mPeerInformation[peerDeviceId].deviceId = gInvalidDeviceId_c;
        PRINTF("BLE Disconnected\r\n");
        if (!handleStopSession((uint8_t)peerDeviceId)) {
            LOG_E("Stopping session failed");
        }
        else {
            LOG_I("BLE Disconnected peer %d, %d peers connected\r\n", peerDeviceId + 1, howManyPeer());
        }
        if (phOsalUwb_ConsumeSemaphore_WithTimeout(mTlvMutex, BLE_CONN_CB_MAX_REENTRANCE_TIMEOUT) !=
            UWBSTATUS_SUCCESS) {
            LOG_E("Ble connection callback timeout");
        }

        /* if not more peer connected, shutdown UWB */
        if (howManyPeer() == 0) {
            if (!handleShutDown()) {
                LOG_E("Stack Deinit failed");
            }
#if defined(cPWR_UsePowerDownMode) && (cPWR_UsePowerDownMode)
            UWBT_PowerModeEnter(UWBT_POWER_DOWN_MODE);
            PWR_AllowDeviceToSleep();
#endif /* cPWR_UsePowerDownMode */
        }
        (void)phOsalUwb_ProduceSemaphore(mTlvMutex);
        /* in any case restart advertising since a connection has been released */
        BleApp_Start();

#if TAG_BUILD_CFG == MANUF_TEST_BUILD || TAG_BUILD_CFG == VALIDATION_V3_BUILD
        bleConnection = FALSE;
#endif
    } break;

#if defined(gAppUsePairing_d) && (gAppUsePairing_d)
    case gConnEvtPairingComplete_c: {
        if (pConnectionEvent->eventData.pairingCompleteEvent.pairingSuccessful) {
            LOG_I("BLE Pairing to peer #%d success\r\n", peerDeviceId + 1);
        }
        else {
            LOG_E("BLE Pairing to peer #%d failed\r\n", peerDeviceId + 1);
        }
    } break;
#endif // defined(gAppUsePairing_d) && (gAppUsePairing_d)

    default:
        break;
    }
}

/*! *********************************************************************************
* \brief        Handles GATT server callback from host stack.
*
* \param[in]    deviceId        Peer device ID.
* \param[in]    pServerEvent    Pointer to gattServerEvent_t.
********************************************************************************** */
static void BleApp_GattServerCallback(deviceId_t deviceId, gattServerEvent_t *pServerEvent)
{
    uint16_t handle;
    uint8_t status;

    switch (pServerEvent->eventType) {
    case gEvtMtuChanged_c: {
        //uint8_t notifMaxPayload = 0;
        //notifMaxPayload = pServerEvent->eventData.mtuChangedEvent.newMtu - mAttNotifHeaderSize_c;
    } break;

    case gEvtAttributeWritten_c: {
        handle = pServerEvent->eventData.attributeWrittenEvent.handle;
        status = gAttErrCodeNoError_c;
        GattServer_SendAttributeWrittenStatus(deviceId, handle, status);
        if (handle == value_qpps_rx) {
            BleApp_ReceivedDataHandler(deviceId,
                pServerEvent->eventData.attributeWrittenEvent.aValue,
                pServerEvent->eventData.attributeWrittenEvent.cValueLength);
        }
    } break;

    case gEvtAttributeWrittenWithoutResponse_c: {
        handle = pServerEvent->eventData.attributeWrittenEvent.handle;

        if (handle == value_qpps_rx) {
            BleApp_ReceivedDataHandler(deviceId,
                pServerEvent->eventData.attributeWrittenEvent.aValue,
                pServerEvent->eventData.attributeWrittenEvent.cValueLength);
        }
    } break;

    case gEvtCharacteristicCccdWritten_c: {
        handle = pServerEvent->eventData.charCccdWrittenEvent.handle;
        if (handle == cccd_qpps_tx) {
            mPeerInformation[deviceId].ntf_cfg = pServerEvent->eventData.charCccdWrittenEvent.newCccd;
        }
    } break;

    case gEvtAttributeRead_c: {
        handle = pServerEvent->eventData.attributeReadEvent.handle;
        LOG_I("Attribute read by peer device \r\n");
        GattServer_SendAttributeReadStatus(deviceId, handle, gAttErrCodeNoError_c);
    } break;

    default:
        break;
    }
}

/*! *********************************************************************************
* \brief        Handles BLE Advertising callback from host stack.
*
* \param[in]    pAdvertisingEvent    Pointer to gapAdvertisingEvent_t.
********************************************************************************** */
static void BleApp_AdvertisingCallback (gapAdvertisingEvent_t* pAdvertisingEvent)
{
    switch (pAdvertisingEvent->eventType)
    {
        case gAdvertisingStateChanged_c:
        {
            mAdvState.advOn = !mAdvState.advOn;

            if(!mAdvState.advOn)
            {
                PRINTF("Advertising stopped.\r\n");
                /* UI */
                LedStopFlashingAllLeds();
                LedStartFlashingAllLeds();
            }
            else
            {
            	PRINTF("Advertising started.\r\n");

                /* UI */
                LedStopFlashingAllLeds();
                Led1Flashing();
            }
        }
        break;

        case gAdvertisingCommandFailed_c:
        {
            /* Panic UI */
            Led2On();
            panic(0,0,0,0);
        }
        break;

        default:
        {
            ; /* No action required */
        }
        break;
    }
}

static void BleApp_ReceivedDataHandler(deviceId_t deviceId, uint8_t *aValue, uint16_t valueLength)
{
    tlvRecv(deviceId, UWB_HIF_BLE, aValue, valueLength);
}

void UWB_Interrupt_ISR_Init(void)
{
#if UWBIOT_UWBD_SR040
    UWBT_InterruptISRInit(UWB_Interrupt_ISR);
//    (void)GpioInputPinInit(&interrupt_pin_config, 1);
    // GpioInstallIsr(tempf, gGpioIsrPrioLow_c, gInt_IsrPrio_c, &interrupt_pin_config);
#endif
}

void UWB_Interrupt_ISR(void)
{
#if UWBIOT_UWBD_SR040
    UWBT_PowerModeEnter(UWBT_RUN_MODE);
    phTmlUwb_helios_irq_enable();
//    if (GpioIsPinCauseOfWakeup(&interrupt_pin_config)) {
//        UWBT_PowerModeEnter(UWBT_RUN_MODE);
//        uint8_t deinitialize_stack[1]           = {kMsg_Stop};
//        phLibUwb_Message_t shareable_config_buf = {0};
//        shareable_config_buf.eMsgType           = 0;
//        shareable_config_buf.Size               = sizeof(deinitialize_stack);
//        shareable_config_buf.pMsgData           = &deinitialize_stack[0];
//        (void)phOsalUwb_msgsnd(tlvMngQueue, &shareable_config_buf, NO_DELAY);
//    }
#endif
}

/*! *********************************************************************************
* @}
********************************************************************************** */

#endif // UWBIOT_APP_BUILD__DEMO_NEARBY_INTERACTION
